from maix import camera, display, image, time, uart, app
from struct import pack

   

white_th = [[60,80, -11, 10, -10, 10]]    
yellow_th = [[34,79,-29,-1,10,70]]
grey_th = [[34,48,-9,14,-5,4]]


red_th = [[0, 100, 16, 127, 5, 127],[0,80,40,80,10,80]]  
green_th = [[0, 80, -120, -10, 0, 30]]   
blue_th = [[0,43,-13,30,-39,-15]]

red_blob_th = [[48,75,42,69,22,50]]
green_blob_th = [[67, 100, -59, -15, 56, 74]]
blue_blob_th = [[45,69,-13,44,-78,-27]]

# red_blob_th = [[0,80,0,80,10,80]]
# green_blob_th = [[0,100,-128,-7,-16,34]]
# blue_blob_th = [[0,60,0,127,-128,-35]]

bin_black_th = [[0,0,0,0,0,0]]
bin_white_th = [[100,100,0,0,0,0]]

blue_garage_th = [[0,43,-12,24,-62,-23]]

roi_all=[0, 0, 160, 120]
    
# blue_th = [[0,36,14,56,-62,-7]]
#-------------------------------------------------------工具函数---------------------------------------------------#
def abs(num):
    if num < 0:
        num = -num
    return num

#-------------------------------------------------------巡线---------------------------------------------------#
def send_track_data(err):
    data = pack("<bbi",
                0x2c,
                0x12,
                err)
    serial.write(data)

def track_func(img):
    blobs = img.find_blobs(yellow_th,pixels_threshold = 1000)
    if blobs:
        ## 找最大的色块，避免误判跳变
        blob_max = blobs[0]
        blob_max_num = blobs[0].pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob
        rect = blob_max.rect()
        # img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
        if all_flag == 0:
            send_track_data(rect[1]+rect[3])  #靠陀螺仪保持方向，只需要直到赛道色块的y坐标就行
        if all_flag == 1:
            send_track_data(rect[1]+rect[3])  #靠陀螺仪保持方向，只需要直到赛道色块的y坐标就行
        string = f"err:{rect[1]+rect[3]}"
        img.draw_string(0, 0, string, image.COLOR_RED,scale = 0.8)
        # if all_flag == 2:
        #     send_yuanpan_data(rect[1]+rect[3],0)  # 找圆盘时，传的东西不一样
        #     print(rect[1]+rect[3])
#-------------------------------------------------------取物料---------------------------------------------------#


def send_blob_data(head,cx,cy,pixel):
    data = pack("<bbbiii",
                0x2c,
                0x12,
                head,
                cx,
                cy,
                pixel)
    serial.write(data)

cx_last = 0
cy_last = 0

blob_time_red = 0
have_find_red = 0

start_time = 0
blob_time = 0

def find_red_blob(img):
    global cx_last,cy_last,have_find_red,blob_time_red,get_index,start_time,blob_time

    start_time += 1
    blobs = img.find_blobs(red_blob_th,pixels_threshold = 1000)
    if blobs:

        ## 找最大的红色色块
        blob_max = blobs[0]
        blob_max_num = blobs[0].pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob

        if (get_index == 0 or get_index == 3) and  start_time < 45:
            while(img.find_blobs(red_blob_th,pixels_threshold = 1000) or blob_time < 700):
                img = cam.read().lens_corr(strength = 1.7)
                blob_time+=1
            start_time = 499
            

        ## 判断转盘是不是还在变
        if have_find_red == 0: 
            if (abs(blob_max.cx() - cx_last) < 5) and (abs(blob_max.cy() - cy_last) < 5):
                blob_time_red += 1; 
                if  blob_time_red > 40:  # 转盘停多久判断为停止
                    have_find_red = 1
                    get_index += 1
            else:
                blob_time_red = 0
                
            send_blob_data(0x01,499,499,0)  # 不管怎么样就发这个

        elif have_find_red == 1:    # 物块停留时间超过一定时间了
            rect = blob_max.rect()
            img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
            string = f"cx:{blob_max.cx()},cy:{blob_max.cy()},pixel:{blob_max.pixels()}"
            img.draw_string(0, 0, string, image.COLOR_RED,scale = 0.8)
            send_blob_data(0x01,blob_max.cx(),blob_max.cy(),blob_max.pixels())
        cx_last = blob_max.cx()
        cy_last = blob_max.cy()
    else:
        send_blob_data(0x01,499,499,0)
        
blob_time_green = 0
have_find_green = 0
def find_green_blob(img):
    global cx_last,cy_last,blob_time_green,have_find_green,get_index,start_time,get_index,start_time,blob_time
    start_time += 1
    blobs = img.find_blobs(green_blob_th,pixels_threshold = 1000)
    if blobs:
        
        ## 找最大的红色色块
        blob_max = blobs[0]
        blob_max_num = blobs[0].pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob

        if (get_index == 0 or get_index == 3) and  start_time < 45:
            while(img.find_blobs(green_blob_th,pixels_threshold = 1000) or blob_time < 700):
                img = cam.read().lens_corr(strength = 1.7)
                blob_time+=1
            start_time = 499

        ## 判断转盘是不是还在变
        if have_find_green == 0: 
            if (abs(blob_max.cx() - cx_last) < 5) and (abs(blob_max.cy() - cy_last) < 5):
                blob_time_green += 1; 
                if  blob_time_green > 40:  # 转盘停多久判断为停止
                    have_find_green = 1
                    get_index += 1
            else:
                blob_time_green = 0
                
            send_blob_data(0x02,499,499,0)  # 不管怎么样就发这个

        elif have_find_green == 1:    # 物块停留时间超过一定时间了
            rect = blob_max.rect()
            img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
            string = f"cx:{blob_max.cx()},cy:{blob_max.cy()},pixel:{blob_max.pixels()}"
            img.draw_string(0, 0, string, image.COLOR_RED,scale = 0.8)
            send_blob_data(0x02,blob_max.cx(),blob_max.cy(),blob_max.pixels())

        cx_last = blob_max.cx()
        cy_last = blob_max.cy()
    else:
        send_blob_data(0x02,499,499,0)

blob_time_blue = 0
have_find_blue = 0
def find_blue_blob(img):
    global cx_last,cy_last,blob_time_blue,have_find_blue,get_index,start_time,get_index,start_time,blob_time
    start_time += 1
    blobs = img.find_blobs(blue_blob_th,pixels_threshold = 1000)
    if blobs:
        ## 找最大的红色色块
        blob_max = blobs[0]
        blob_max_num = blobs[0].pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob

        if (get_index == 0 or get_index == 3) and  start_time < 45:
            while(img.find_blobs(blue_blob_th,pixels_threshold = 1000) or blob_time < 700):
                img = cam.read().lens_corr(strength = 1.7)
                blob_time+=1
            start_time = 499

        ## 判断转盘是不是还在变
        if have_find_blue == 0: 
            if (abs(blob_max.cx() - cx_last) < 5) and (abs(blob_max.cy() - cy_last) < 5):
                blob_time_blue += 1; 
                if  blob_time_blue > 40:  # 转盘停多久判断为停止
                    have_find_blue = 1
                    get_index += 1
            else:
                blob_time_blue = 0
                
            send_blob_data(0x03,499,499,0)  # 不管怎么样就发这个

        elif have_find_blue == 1:    # 物块停留时间超过一定时间了
            rect = blob_max.rect()
            img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
            string = f"cx:{blob_max.cx()},cy:{blob_max.cy()},pixel:{blob_max.pixels()}"
            img.draw_string(0, 0, string, image.COLOR_RED,scale = 0.8)
            send_blob_data(0x03,blob_max.cx(),blob_max.cy(),blob_max.pixels())

        cx_last = blob_max.cx()
        cy_last = blob_max.cy()
    else:
        send_blob_data(0x03,499,499,0)

# def send_yuanpan_data(err,point):
#     data = pack("<bbii",
#                 0x2c,
#                 0x12,
#                 err,
#                 point)
#     serial.write(data)

# def find_zhuanpan(img):
#     blobs = img.find_blobs(white_th,pixels_threshold = 3000,roi = [0,0,160,85])
#     if blobs:
#         blob_max = blobs[0]
#         blob_max_num = blob_max.pixels()
#         for blob in blobs:
#             if blob.pixels() > blob_max_num :
#                 blob_max = blob
#         ## 判断是不是转盘的白色
#         ## 外接矩形的长最大时，就算有物块，右上角的点，和h不变
#         if (blob_max.rect()[3] > 40) : 
#             rect = blob_max.rect()
#             img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
#             # print(blob_max.cx(),blob_max.cy(),rect[3])
#             # print(blob_max.pixels())
#             x_point = blob_max.rect()[0]+ blob_max.rect()[2]  
#             send_yuanpan_data(blob_max.rect()[3],x_point)
#             print("h",blob_max.rect()[3],"x_point",x_point)
#         else:
#             track_func(img)
#     else:
#         track_func(img)

#-------------------------------------------------------找色环---------------------------------------------------#
def send_circle_data(dif,x,y):
    data = pack("<bbbii",
                0x2c,
                0x12,
                dif,
                x,
                y)
    serial.write(data)


near_green_flag = 0
def find_green_circle(img): 
    global  near_green_flag,all_flag
    img.binary(thresholds = green_th)
    
    img.dilate(2,0)
    img.dilate(2,0)
    img.erode(2,-1)
    inside_point = [0,0]
    pixels = 0
    if near_green_flag == 0:
        blobs = img.find_blobs(bin_white_th,pixels_threshold = 2000)
        # blobs = img.find_blobs(bin_white_th,roi = [0,0,160,60],pixels_threshold = 2000)
        if blobs:
            blob_max = blobs[0]
            blob_max_num = blob_max.pixels()
            for blob in blobs:
                if blob.pixels() > blob_max_num :
                    blob_max = blob
            rect = blob_max.rect()
            img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
            print(rect)
            if blob_max.pixels() > 2500:  # 测试出来的，表示接近圆环了
                near_green_flag = 1
            else:
                send_circle_data(0x02,499,499)
        else:
            send_circle_data(0x02,499,499)   # 传一个不可能的数字
    else:
        blobs = img.find_blobs(bin_black_th,pixels_threshold = 400)
        if blobs:
            blob_min_num = 19200
            blob_min = blobs[0]
            for blob in blobs:
                if blob.pixels() < blob_min_num :
                    blob_min = blob
            if blob_min.pixels() < 10000:
                rect = blob_min.rect()
                img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
                inside_point[0] = blob_min.cx()
                inside_point[1] = blob_min.cy()
                send_circle_data(0x02,inside_point[0],inside_point[1])
                img.draw_cross(inside_point[0],inside_point[1],color=image.COLOR_RED)
            else:
                send_circle_data(0x02,499,499)   # 传一个不可能的数字


near_red_flag = 0
def find_red_circle(img): 
    global  near_red_flag,all_flag
    img.binary(thresholds = red_th)
    
    img.dilate(2,0)
    img.dilate(2,0)
    img.erode(2,-1)
    inside_point = [0,0]
    pixels = 0
    if near_red_flag == 0:
        blobs = img.find_blobs(bin_white_th,pixels_threshold = 2000)
        # blobs = img.find_blobs(bin_white_th,roi = [0,0,160,60],pixels_threshold = 2000)
        if blobs:
            blob_max = blobs[0]
            blob_max_num = blob_max.pixels()
            for blob in blobs:
                if blob.pixels() > blob_max_num :
                    blob_max = blob
            rect = blob_max.rect()
            img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
            # print(rect)
            if blob_max.pixels() > 2500:  # 测试出来的，表示接近圆环了
                near_red_flag = 1
            else:
                send_circle_data(0x01,499,499)
        else:
            send_circle_data(0x01,499,499)   # 传一个不可能的数字
    else:
        blobs = img.find_blobs(bin_black_th,pixels_threshold = 400)
        if blobs:
            blob_min_num = 19200
            blob_min = blobs[0]
            for blob in blobs:
                if blob.pixels() < blob_min_num :
                    blob_min = blob
            if blob_min.pixels() < 10000:
                rect = blob_min.rect()
                img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
                inside_point[0] = blob_min.cx()
                inside_point[1] = blob_min.cy()
                send_circle_data(0x01,inside_point[0],inside_point[1])
                img.draw_cross(inside_point[0],inside_point[1],color=image.COLOR_RED)
            else:
                send_circle_data(0x03,499,499)   # 传一个不可能的数字

        


near_blue_flag = 0
def find_blue_circle(img): 
    global  near_blue_flag,all_flag
    img.binary(thresholds = blue_th)
    
    img.dilate(2,0)
    img.dilate(2,0)
    img.erode(2,-1)
    inside_point = [0,0]
    pixels = 0
    if near_blue_flag == 0:
        blobs = img.find_blobs(bin_white_th,pixels_threshold = 2000)
        # blobs = img.find_blobs(bin_white_th,roi = [0,0,160,60],pixels_threshold = 2000)
        if blobs:
            blob_max = blobs[0]
            blob_max_num = blob_max.pixels()
            for blob in blobs:
                if blob.pixels() > blob_max_num :
                    blob_max = blob
            rect = blob_max.rect()
            img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
            print(rect)
            if blob_max.pixels() > 2500:  # 测试出来的，表示接近圆环了
                near_blue_flag = 1
            else:
                send_circle_data(0x03,499,499)
        else:
            send_circle_data(0x03,499,499)   # 传一个不可能的数字
    else:
        blobs = img.find_blobs(bin_black_th,pixels_threshold = 400)
        if blobs:
            blob_min_num = 19200
            blob_min = blobs[0]
            for blob in blobs:
                if blob.pixels() < blob_min_num :
                    blob_min = blob
            if blob_min.pixels() < 10000:
                rect = blob_min.rect()
                img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
                inside_point[0] = blob_min.cx()
                inside_point[1] = blob_min.cy()
                send_circle_data(0x03,inside_point[0],inside_point[1])
                img.draw_cross(inside_point[0],inside_point[1],color=image.COLOR_RED)
            else:
                send_circle_data(0x03,499,499)   # 传一个不可能的数字





def find_red_blob_circle(img):

    blobs = img.find_blobs(red_blob_th,pixels_threshold = 2000)
    if blobs:
        ## 找最大的红色色块
        blob_max = blobs[0]
        blob_max_num = blobs[0].pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob

        ## 判断转盘是不是还在变
        rect = blob_max.rect()
        img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
        string = f"cx:{blob_max.cx()},cy:{blob_max.cy()}"
        img.draw_string(0, 0, string, image.COLOR_RED,scale = 0.8)
        send_circle_data(0x01,blob_max.cx(),blob_max.cy())
    else:
        send_circle_data(0x01,499,499)

def find_green_blob_circle(img):

    blobs = img.find_blobs(green_blob_th,pixels_threshold = 2000)
    if blobs:
        ## 找最大的红色色块
        blob_max = blobs[0]
        blob_max_num = blobs[0].pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob

        ## 判断转盘是不是还在变
        rect = blob_max.rect()
        img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
        string = f"cx:{blob_max.cx()},cy:{blob_max.cy()}"
        img.draw_string(0, 0, string, image.COLOR_RED,scale = 0.8)
        send_circle_data(0x02,blob_max.cx(),blob_max.cy())
    else:
        send_circle_data(0x02,499,499)

def find_blue_blob_circle(img):

    blobs = img.find_blobs(blue_blob_th,pixels_threshold = 2000)
    if blobs:
        ## 找最大的红色色块
        blob_max = blobs[0]
        blob_max_num = blobs[0].pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob

        ## 判断转盘是不是还在变
        rect = blob_max.rect()
        img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
        string = f"cx:{blob_max.cx()},cy:{blob_max.cy()}"
        img.draw_string(0, 0, string, image.COLOR_RED,scale = 0.8)
        send_circle_data(0x03,blob_max.cx(),blob_max.cy())
    else:
        send_circle_data(0x03,499,499)


#--------------------------------------------cross-----------------------------------------------#
def send_garage_data(cx,cy):
    data = pack("<bbii",
                0x2c,
                0x12,
                cx,
                cy,)
    serial.write(data)
    
def garage(img):
    global have_found
    blobs = img.find_blobs(blue_garage_th,merge = True,margin = 10,pixels_threshold = 500)
    if blobs:
        blob_max = blobs[0]
        blob_max_num = blob_max.pixels()
        for blob in blobs:
            if blob.pixels() > blob_max_num :
                blob_max = blob
        rect = blob_max.rect()
        img.draw_rect(rect[0], rect[1], rect[2], rect[3],image.COLOR_RED)
        send_garage_data(rect[0],rect[1])
        print(rect[0],rect[1])
    else:
        send_garage_data(499,499)

    
        # print(0)



# 高斯滤波找圆环 帧率不如侵蚀膨胀
# def asd(img):
#     img.gaussian(2)
#     circles = img.find_circles(threshold = 3000)
#     for a in circles:1
#         img.draw_circle(a.x(), a.y(), a.r(),image.COLOR_RED, 2)




#####################################################################################################################################################
# all_flag = 0   # 总标志位
all_flag = 0  # 总标志位
put_flag = 0
process_flag = 0
get_blob_flag = 0  # 找色块标志位
get_index = 0   # 抓取标志位

# put_order[]

# 回应单片机自己接收到的值，切换任务完毕
def respond(data):
    data = pack("<bbb",
                0x55,
                0x55,
                data)
    serial.write(data)

def on_received(serial : uart.UART, data : bytes):
    global all_flag,near_blue_flag,near_red_flag,near_green_flag,cx_last,cy_last,have_found,blob_time
    global blob_time_blue,have_find_blue,blob_time_green,have_find_green,blob_time_red,have_find_red,start_time
    # 通过帧头来同步通信，保证不错位
    if data[0] == 0x55 and data[1] == 0x55:
        all_flag = data[2]
        respond(data[2])
        # 进来就清一次，无所谓
        near_blue_flag = 0 
        near_red_flag = 0
        near_green_flag = 0
        cx_last = 0
        cy_last = 0
        
        blob_time_blue = 0
        have_find_blue = 0

        blob_time_green = 0
        have_find_green = 0

        blob_time_red = 0
        have_find_red = 0

        have_found = 0

        start_time = 0
        blob_time = 0

        

device = "/dev/ttyS0"
serial = uart.UART(device, 115200)

serial.set_received_callback(on_received)  # 设置串口回调函数

disp = display.Display()        # 屏幕初始化
cam = camera.Camera(160, 120)   # 摄像头初始化
cam.skip_frames(30)             # 跳过起始的30帧图像

#让单片机直到自己初始化完了
# while(all_flag==0):
#     respond(0x66)
#     time.sleep_ms(20)

while not app.need_exit():
    img = cam.read().lens_corr(strength = 1.7)      # 处理鱼眼畸变1.7 (目标检测最好在摄像头中间)
    # 任务总处理,车出库(写死)，直到检测到黄色边界达到设定的值
    # 标志 0x2c 0x12

    if all_flag == 0:
        track_func(img)
    elif all_flag == 1:
        track_func(img)
    elif all_flag == 8 or all_flag == 20 or all_flag == 33: 
        find_red_circle(img)
    elif all_flag == 9 or all_flag == 21 or all_flag == 34: 
        find_green_circle(img)
    elif all_flag == 10 or all_flag == 22 or all_flag == 35:
        find_blue_circle(img)

    elif all_flag == 11 or all_flag == 36  or all_flag == 45:
        find_red_blob_circle(img) 
    elif all_flag == 12 or all_flag == 37  or all_flag == 46:
        find_green_blob_circle(img) 
    elif all_flag == 13 or all_flag == 38 or all_flag == 47:
        find_blue_blob_circle(img) 


    elif all_flag == 60 or all_flag == 63:
        find_red_blob(img)
    elif all_flag == 61 or all_flag == 64:
        find_green_blob(img)
    elif all_flag == 62 or all_flag == 65:
        find_blue_blob(img)

    elif all_flag == 52:
        garage(img)

    img.draw_string(0, 100, f"{all_flag}", image.COLOR_RED,scale = 0.8)
    disp.show(img)
    # print(all_flag)
    # fps = time.fps()            # Calculate FPS between last time fps() call and this time call.
    # print(f"time: {1000/fps:.02f}ms, fps: {fps:.02f}") # print FPS in console

